using System;
using System.Collections.Generic;
using System.Text;
using G3.State;
using dnAnalytics.LinearAlgebra;
using System.IO;

namespace G3.AI
{
    class Evolution : IDriver
    {
        private const int POPULATION = 20; //100 is optimal
        private const int GENERATIONS = 5; //UNUSED
        private const int ELITES = 5; //20 is optimal
        private const int HALF_VECTORSIZE = 3;
        private const double SIMULATED_TIMESPAN = 40.0; //seconds //80.0 is optimal in Hungaroring
        private const double SIMULATION_STEP = 100.0; //milliseconds
        private const double DISTANCE_WEIGHT = -0.1;
        private const double TIME_PENALTY_WEIGHT = 4.0;
        private const double DISTANCE_PENALTY_WEIGHT = 2.0;
        private const double UPPER_BOUND = 50.0;
        private const double LOWER_BOUND = -10.0;
        private const double MUTATION_PROBABILITY = 0.1;

        private Random rnd;
        private List<EvolAgent> agents;

        private StreamWriter sw;

        public Evolution()
        {
            sw = new StreamWriter("EvolLog.txt");
            rnd = new Random();
            agents = new List<EvolAgent>(POPULATION);
            for(int i=0; i<POPULATION; ++i)
            {
                double[] vector = new double[2 * HALF_VECTORSIZE];
                for (int j=0; j < 2 * HALF_VECTORSIZE; ++j)
                {
                    vector[j] = randomBetween(LOWER_BOUND, UPPER_BOUND);
                }
                agents.Add(new EvolAgent(vector, HALF_VECTORSIZE));
            }
            generateFitnesses();
        }

        private double randomBetween(double d1, double d2)
        {
            if (d1 > d2)
            {
                double d = d2;
                d2 = d1;
                d1 = d;
            }
            return d1 + (d2 - d1) * rnd.NextDouble();
        }

        private double randomBetweenAndOver(double d1, double d2)
        {
            if (d1 == d2) return randomBetween(LOWER_BOUND, UPPER_BOUND);
            if (d1 > d2)
            {
                double d = d2;
                d2 = d1;
                d1 = d;
            }
            return d1 - (d2 - d1) + 3 * (d2 - d1) * rnd.NextDouble();
        }

        private double[] crossBreed(double[] v1, double[] v2)
        {
            double[] v = new double[2 * HALF_VECTORSIZE];
            for (int i = 0; i < 2 * HALF_VECTORSIZE; ++i)
            {
                v[i] = randomBetweenAndOver(v1[i], v2[i]);
                if (rnd.NextDouble() < MUTATION_PROBABILITY)
                {
                    //double distance = Math.Abs(v1[i] - v2[i]);
                    v[i] = randomBetween(LOWER_BOUND, UPPER_BOUND);
                }
            }
            return v;
        }

        private void newGeneration()
        {
            agents.RemoveRange(ELITES, POPULATION - ELITES);
            for (int i = ELITES; i < POPULATION; ++i)
            {
                agents.Add(new EvolAgent(crossBreed(agents[rnd.Next(0, ELITES)].getVector(), agents[rnd.Next(0, ELITES)].getVector()), HALF_VECTORSIZE));
                //agents.Add(new EvolAgent(perturbate(agents[rnd.Next(0,ELITES)].getVector()), HALF_VECTORSIZE));
            }
        }

        private void generateFitnesses()
        {
            for (int i = 0; i < POPULATION; ++i)
            {
                Controller.Controller.GetOnlyInstance().AttachedState.Reset();
                while (Controller.Controller.GetOnlyInstance().AttachedState.Time < SIMULATED_TIMESPAN)
                {
                    Pair response = getOpinion(i);
                    Controller.Controller.GetOnlyInstance().AttachedState.Wheel = response.x;
                    Controller.Controller.GetOnlyInstance().AttachedState.Throttle = response.y;
                    Controller.Controller.GetOnlyInstance().AttachedState.Simulate(SIMULATION_STEP);
                }
                agents[i].fitness = DISTANCE_WEIGHT * Controller.Controller.GetOnlyInstance().AttachedState.Distance +
                                    DISTANCE_PENALTY_WEIGHT * Controller.Controller.GetOnlyInstance().AttachedState.DistancePenalty +
                                    TIME_PENALTY_WEIGHT * Controller.Controller.GetOnlyInstance().AttachedState.TimePenalty;
            }
            Controller.Controller.GetOnlyInstance().AttachedState.Reset();
            agents.Sort();
            for (int i = 0; i < POPULATION; ++i)
            {
                sw.Write(agents[i].fitness.ToString()+";");
                //sw.WriteLine(i.ToString()+".agent: "+str(agents[i].getVector())+" with fitness:"+agents[i].fitness.ToString());
            }
            sw.WriteLine();
        }

        private String str(double[] v)
        {
            String s = "";
            //for (int i = 0; i < HALF_VECTORSIZE * 2; ++i)
            //{
            //    s += v[i].ToString()+";";
            //}
            return s;
        }

        private Pair getOpinion(int i)
        {
            Vector points = Controller.Map.GetControlPoints(Controller.Map.StateCoordsToMapCoords(Controller.Controller.GetOnlyInstance().AttachedState.CarPosition), Controller.Controller.GetOnlyInstance().AttachedState.CarOrientationRadian, 7);
            double[] v = new double[HALF_VECTORSIZE];
            v[0] = points[0];
            v[1] = points[6];
            Pair input = agents[i].Think(v, Controller.Controller.GetOnlyInstance().AttachedState.CarSpeed);
            return input;
        }

        public Pair Think()
        {
            Pair input = getOpinion(0);
            if (input.x < -100.0) input.x = -100.0;
            if (input.x > 100.0) input.x = 100.0;
            if (input.y < -100.0) input.y = -100.0;
            if (input.y > 100.0) input.y = 100.0;
            Controller.Controller.GetOnlyInstance().SetWheelThrootlePositions(input.y, input.x);
            return input;
        }

        public void Teach(int generations)
        {
            for (int i = 0; i < generations; ++i)
            {
                //sw.WriteLine("Generation: " + i.ToString());
                newGeneration();
                generateFitnesses();
            }
        }

        public DriverType Type()
        {
            return DriverType.RuleBased;
        }

        public void LoadProfile(String fileName)
        {
            StreamReader loadFile = new StreamReader(fileName);
            agents.Clear();
            while (loadFile.Peek() >= 0 && agents.Count<POPULATION)
            {
                String line = loadFile.ReadLine();
                char[] delim = new char[] { ';' };
                String[] splitted = line.Split(delim);
                double[] vector = new double[2 * HALF_VECTORSIZE];
                for (int j = 0; j < 2 * HALF_VECTORSIZE; ++j)
                {
                    vector[j] = double.Parse(splitted[j]);
                }
                agents.Add(new EvolAgent(vector, HALF_VECTORSIZE));
            }
            loadFile.Close();
            while (agents.Count < POPULATION)
            {
                double[] vector = new double[2 * HALF_VECTORSIZE];
                for (int j = 0; j < 2 * HALF_VECTORSIZE; ++j)
                {
                    vector[j] = randomBetween(LOWER_BOUND, UPPER_BOUND);
                }
                agents.Add(new EvolAgent(vector, HALF_VECTORSIZE));
            }
            generateFitnesses();
            loadFile.Close();
        }

        public void SaveProfile(String fileName)
        {
            StreamWriter saveFile = new StreamWriter(fileName);
            for (int i = 0; i < POPULATION; ++i)
            {
                for (int j = 0; j < 2 * HALF_VECTORSIZE; ++j)
                {
                    saveFile.Write((agents[i].getVector())[j].ToString() + ";");
                }
                saveFile.Write("\n");
            }
            saveFile.Close();
        }
    }
}
